/*
 * ContextLearner.cpp
 *
 *  Created on: May 24, 2011
 *      Author:
 *      Johan Granberg
 *      Alper Aydemir
 *
 * TODO: Omitted computing norm_zenith
 *       Compute distance to object -- Do it better
 *       FeatureResponse
 */

#include "ContextLearner.h"
#include "ContextLearnerUtils.h"
#include <pthread.h>
#include <stdlib.h>

#include <sys/time.h>
#include <sys/types.h>
#include <dirent.h>

#include <time.h>

#include <fstream>
#include <iostream>
#include <stdio.h>

#include <string.h>
#include <math.h>
#include <vector>
#include "FeaturePatch.cpp"

#include <math.h>
#include <stdio.h>
#include <sys/time.h>
#include <iostream>
#include <time.h>
#include <fstream>
#include <stdlib.h>

#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/ap.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/alglibinternal.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/linalg.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/statistics.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/alglibmisc.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/specialfunctions.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/solvers.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/optimization.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/dataanalysis.cpp"


using namespace std;

namespace findobject
{
void
show_matrix(float ** mat, int sx, int sy)
{
	IplImage * work_image = 0;
	float min = 10000000;
	float max = -1000000;
	for (int i = 0; i < sx; i++)
	{
		for (int j = 0; j < sy; j++)
		{
			if (mat[i][j] != -123)
			{
				if (mat[i][j] < min)
				{
					min = mat[i][j];
				}
				else if (mat[i][j] > max)
				{
					max = mat[i][j];
				}
			}
		}
	}

	float mul = 255.0f / (max - min);
	if (work_image != 0)
		cvReleaseImage(&work_image);
	work_image = cvCreateImage(cvSize(sx, sy), IPL_DEPTH_8U, 3);
	for (int i = 0; i < sx; i++)
	{
		for (int j = 0; j < sy; j++)
		{
			if (mat[i][j] != -123)
			{
				int val = (mat[i][j] - min) * mul;
				work_image->imageData[3 * (j * sx + i) + 0] = val;
				work_image->imageData[3 * (j * sx + i) + 1] = val;
				work_image->imageData[3 * (j * sx + i) + 2] = val;
			}
			else
			{
				work_image->imageData[3 * (j * sx + i) + 0] = 0;
				work_image->imageData[3 * (j * sx + i) + 1] = 0;
				work_image->imageData[3 * (j * sx + i) + 2] = 0;
			}
		}
	}
	//cvNamedWindow("black and white image", CV_WINDOW_AUTOSIZE);
	//cvShowImage("black and white image", work_image);
	//cvWaitKey(100);
}

void
save_matrix(char * filename,float ** mat, int sx, int sy)
{
	IplImage * work_image = 0;
	float min = 10000000;
	float max = -1000000;
	for (int i = 0; i < sx; i++)
	{
		for (int j = 0; j < sy; j++)
		{
			if (mat[i][j] != -123)
			{
				if (mat[i][j] < min)
				{
					min = mat[i][j];
				}
				else if (mat[i][j] > max)
				{
					max = mat[i][j];
				}
			}
		}
	}

	float mul = 255.0f / (max - min);
	if (work_image != 0)
		cvReleaseImage(&work_image);
	work_image = cvCreateImage(cvSize(sx, sy), IPL_DEPTH_8U, 3);
	for (int i = 0; i < sx; i++)
	{
		for (int j = 0; j < sy; j++)
		{
			if (mat[i][j] != -123)
			{
				int val = (mat[i][j] - min) * mul;
				work_image->imageData[3 * (j * sx + i) + 0] = val;
				work_image->imageData[3 * (j * sx + i) + 1] = val;
				work_image->imageData[3 * (j * sx + i) + 2] = val;
			}
			else
			{
				work_image->imageData[3 * (j * sx + i) + 0] = 0;
				work_image->imageData[3 * (j * sx + i) + 1] = 0;
				work_image->imageData[3 * (j * sx + i) + 2] = 0;
			}
		}
	}
	//cvNamedWindow("black and white image", CV_WINDOW_AUTOSIZE);
	//cvShowImage("black and white image", work_image);
	cvSaveImage(filename,work_image);
}

int round(float r) {
	return (r > 0.0) ? floor(r + 0.5) : ceil(r - 0.5);
}

/**
 * Evaluates the likelihood density of an image containing the object
 * that the area can contain the object.
 * @cl: Classifier used to evaluate a patch
 * @iifeature: Feature used for evaluation.
 **/

float ** ContextLearner::evaluate_image(Classifier * cl, IntegralImageFeature * iifeature,IplImage* rgbImage, IplImage* depthImage, int widthStep, int heightStep){

	int img_width  = depthImage->width;
	int img_height = depthImage->height;

	float ** mat = new float*[img_width];
	float ** mat_div = new float*[img_width];

	for (int i = 0; i < img_width; i++){
		mat[i] = new float[img_height];
		mat_div[i] = new float[img_height];
		for (int j = 0; j < img_height; j++){
			mat_div[i][j] = 0;
			mat[i][j] = 0;
		}
	}


	struct timeval start, end;
	long mtime, seconds, useconds; mtime = 0;


	log(0, "Getting XYZ point cloud.");
	PointCloud* pointCloud = getXYZFromDepthImage(depthImage);
	log(0, "Computing point-wise normals");
	setRGB(pointCloud,rgbImage);
	pointCloud->fillNormals();
	pointCloud->setValidPoints();

	gettimeofday(&start, NULL);
	iifeature->reset(pointCloud->m_points);
	//cvNamedWindow("Eval sliding box image", CV_WINDOW_AUTOSIZE);

	for(int w = 0; w < img_width; w+=widthStep){
		for(int h = 0; h < img_height; h+=heightStep)
		{
			float distance_to_center = pointCloud->m_points[w][h].z;
			if(distance_to_center!=0)
			{
				float item_size = (70.0f/0.048329)*target_object_size/max(distance_to_center,0.01f);
				float top_w = w-(0.5f*item_size);
				float top_h = h-(0.5f*item_size);
				float bot_w = w+(0.5f*item_size);
				float bot_h = h+(0.5f*item_size);
				//IplImage* rgbImageClone=cvCloneImage(depthImage);
				FeatureResponse * response = cl->evaluate(iifeature,w, h,item_size);
				if(response == NULL){
					//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(255, 255, 0, 0), CV_FILLED, 8, 0);
				}
				else{
					//FeaturePatch * this_patch = cl->patches[0];
					top_w = min(max(top_w,0.0f),(float)img_width-1);
					top_h = min(max(top_h,0.0f),(float)img_height-1);
					bot_w = min(max(bot_w,0.0f),(float)img_width-1);
					bot_h = min(max(bot_h,0.0f),(float)img_height-1);
					float value = response->response[0];
					if (!isnan(value))
					{
						for (int i = top_w; i < bot_w; i++)
						{
							for (int j = top_h; j < bot_h; j++)
							{
								mat[i][j] += value;
								mat_div[i][j]++;
							}
						}
					}
					delete response;
					//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(value*255, value*255, value*255, 0), CV_FILLED, 8, 0);
					//cvRectangle(rgbImageClone, cvPoint(round(tmp_top_w), round(tmp_top_h)), cvPoint(round(tmp_bot_w), round(tmp_bot_h)),cvScalar(255, 0, 255, 0), 2, 8, 0);
				}

				//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(255, 255, 0, 0), CV_FILLED, 8, 0);
				//cvShowImage("Eval sliding box image", rgbImageClone);
				//cvWaitKey(0);
				//cvReleaseImage(&rgbImageClone);
			}
		}
	}


	gettimeofday(&end, NULL);
	seconds = end.tv_sec - start.tv_sec;
	useconds = end.tv_usec - start.tv_usec;
	mtime = seconds * 1000000 + useconds;
	if (useconds < 0){
		useconds += 1000000;
		seconds--;
	}
	//printf("Elapsed time total : %ld.%ld sec\n", seconds,useconds);
	for (int i = 0; i < img_width; i++){
		for (int j = 0; j < img_height; j++){
			if(mat_div[i][j] > 0){
				mat[i][j] /= mat_div[i][j];
			}
			else{
				mat[i][j] = 0;
			}
		}
	}
	for (int i = 0; i < img_width; i++){
		delete mat_div[i];
	}
	delete mat_div;
	delete pointCloud;

	return mat;
}

/*
Reduces the dimensionality of the data by pca
 */
using namespace alglib;
real_1d_array s2;
real_2d_array v;
void ContextLearner::apply_pca_basis(FeatureResponse * feature_response,int dims)
{
	/*float * new_arr = new float[dims];
	for(int i = 0; i < dims; i++)
	{
		new_arr[i] = 0;
		for(int j = 0; j < feature_response->length;j++)
		{
			new_arr[i] += v[i][j]*feature_response->response[j];
		}
		new_arr[i]*=s2[i];
	}
	delete feature_response->response;
	feature_response->length = dims;
	feature_response->response = new_arr;*/
}

void ContextLearner::reduce_dimensionality(FeatureResponse **& target, FeatureResponse **& data, int nr_data)
{

	/*
Principal components analysis

Subroutine  builds  orthogonal  basis  where  first  axis  corresponds  to
direction with maximum variance, second axis maximizes variance in subspace
orthogonal to first axis and so on.

It should be noted that, unlike LDA, PCA does not use class labels.

INPUT PARAMETERS:
    X           -   dataset, array[0..NPoints-1,0..NVars-1].
                    matrix contains ONLY INDEPENDENT VARIABLES.
    NPoints     -   dataset size, NPoints>=0
    NVars       -   number of independent variables, NVars>=1

    Info        -   return code:
	 * -4, if SVD subroutine haven't converged
	 * -1, if wrong parameters has been passed (NPoints<0,
                          NVars<1)
	 *  1, if task is solved
    S2          -   array[0..NVars-1]. variance values corresponding
                    to basis vectors.
    V           -   array[0..NVars-1,0..NVars-1]
                    matrix, whose columns store basis vectors.

  -- ALGLIB --
     Copyright 25.08.2008 by Bochkanov Sergey
	 */

	printf("---------------------------------------\nreducing dimensionality\n-------------------------------------------\n");
	real_2d_array x;
	x.setlength(nr_data, data[0]->length);
	for(int i = 0; i < nr_data; i++)
	{
		for(int j = 0; j < data[0]->length; j++)
		{
			x[i][j]=data[i]->response[j];
		}
	}
	ae_int_t npoints = nr_data;
	ae_int_t nvars = data[0]->length;
	printf("set...\n");
	ae_int_t info;
	pcabuildbasis(x,npoints, nvars,info,s2,v);
	printf("basis built...\n");
	//Normalize eigenvalues
	float max = 0;
	for(int i = 0 ; i < nvars; i++)
	{
		if(fabs(s2[i])>max)
		{
			max = fabs(s2[i]);
		}
	}
	for(int i = 0 ; i < nvars; i++)
	{
		s2[i]/=max;
	}
	printf("info = %i\n eigenvalues: \n",(int)info);
	for(int i = 0 ; i < nvars; i++)
	{
		printf("%i : %f ev: -> [%f",i,s2[i],v[i][0]);
		for(int j = 1; j < nvars; j++)
		{
			printf(", %f",v[i][j]);
		}
		printf("]\n");
	}
	for(int i = 0; i < npoints; i++)
	{
		apply_pca_basis(data[i],1);
	}
	//out_target = target;
	//out_data = data;
}

/**
Clusters the data.
 */
void
cluster(FeatureResponse ** target, FeatureResponse ** data, FeatureResponse **& out_target, FeatureResponse **& out_data, float ** data_weights, int nr_data,
		int nr_clusters)
{
	printf("\nClustering %d samples each with %d dimensions \n", nr_data, data[0]->length);
	cv::Mat samples = cv::Mat_<float>(nr_data, data[0]->length);
	for (int i = 0; i < nr_data; i++)
	{
		for (int j = 0; j < data[0]->length; j++)
		{
			samples.at<float> (i, j) = data[i]->response[j];
		}
	}

	cv::Mat labels, centers;
	cv::TermCriteria tc = cv::TermCriteria();
	double compactness = cv::kmeans(samples, nr_clusters, labels, tc, 1, cv::KMEANS_PP_CENTERS, &centers);

	if (centers.rows == 0){throw "No clusters found, throwing exception";}
	float * weights = new float[nr_clusters];
	float * value = new float[nr_clusters];
	for (int i = 0; i < nr_clusters; i++){
		weights[i] = 0;
		value[i] = 0;
	}
	for (int i = 0; i < labels.rows; ++i)
	{
		weights[labels.at<int> (0, i)]++;
		value[labels.at<int> (0, i)] += target[i]->response[0];
	}
	for (int i = 0; i < nr_clusters; i++)
	{
		value[i] /= (float) weights[i];
	}

	out_target = new FeatureResponse*[nr_clusters];
	out_data = new FeatureResponse*[nr_clusters];

	for (int i = 0; i < centers.rows; ++i)
	{
		float * tmp_out_target = new float[1];
		tmp_out_target[0] = value[i];
		out_target[i] = new FeatureResponse(tmp_out_target, 1);

		float * tmp_out_data = new float[centers.cols];
		out_data[i] = new FeatureResponse(tmp_out_data, centers.cols);

		for (int j = 0; j < centers.cols; j++)
		{
			tmp_out_data[j] = centers.at<float> (j, i);
		}
	}
	for (int k = 0; k < nr_clusters; k++)
	{
		float ** tmp_holder = new float*[data[0]->length];
		for (int dimcounter = 0; dimcounter < data[0]->length; dimcounter++)
		{
			tmp_holder[dimcounter] = new float[(int) weights[k]];
		}
		int printed = 0;
		for (int i = 0; i < nr_data; i++)
		{
			if (labels.at<int> (0, i) == k)
			{
				for (int j = 0; j < data[0]->length; j++)
				{
					float * tmparray = tmp_holder[j];
					tmparray[printed] = data[i]->response[j];
				}
				printed++;
			}
		}
		weights[k] = ((float) (weights[k] * nr_clusters)) / (float) nr_data, value[k];
		for (int j = 0; j < data[0]->length; j++)
		{
			float * tmparray = tmp_holder[j];
			float thismedian = quick_select(tmparray, weights[k]);
			out_data[k]->response[j] = thismedian;
			delete tmparray;
		}
		delete tmp_holder;

	}
	*data_weights = weights;
	delete value;
}

ContextLearner::ContextLearner()
{
	// TODO Auto-generated constructor stub
	m_classifier = new SVMClassifier;
}

/* --------------------------------------------------------------------------*/
void
ContextLearner::setTrainingObject(std::string objectLabel)
{
	m_objectLabel = objectLabel;
}

/* --------------------------------------------------------------------------*/
void
ContextLearner::setFeatureTypes(int featureFlags)
{
	log(0, "Setting feature types %d", featureFlags);
	m_compositeFeature = new CompositFeature(featureFlags);
	m_IntegralImageFeature = new IntegralImageGeneralFeature(NULL, m_compositeFeature, 0, 0);
	m_numberofObjectPixels = new IntegralImageValue(NULL, 0, 0);
	log(0, "Feature types set.");
}
/* --------------------------------------------------------------------------*/
bool
ContextLearner::setTrainingFolders(std::vector<std::string> trainingFolderPaths)
{
	m_trainingFolderPaths = trainingFolderPaths;
	//TODO: Loop through all the folders and imags and call setTrainingImages function
}

/* --------------------------------------------------------------------------*/
bool
ContextLearner::addTrainingImages(std::string folderPath, vector<int> frameNumbers)
{
	//TODO: Add Images
	trainingImageSet t;
	for (int i = 0; i < frameNumbers.size(); i++)
	{
		char filename[256];
		sprintf(filename, "%sframe%d_rgb.bmp", folderPath.c_str(), frameNumbers[i]);
		log(0, "Adding %s to training images set", filename);
		t.filePathRGB = filename;

		sprintf(filename, "%sframe%d_depth.bmp", folderPath.c_str(), frameNumbers[i]);
		t.filePathDepth = filename;
		log(0, "Adding %s to training images set", filename);

		sprintf(filename, "%s%s/%s_frame%d_mask.bmp", folderPath.c_str(), m_objectLabel.c_str(), m_objectLabel.c_str(), frameNumbers[i]);
		t.filePathMask = filename;
		log(0, "Adding %s to training images set", filename);
		m_trainingImagePaths.push_back(t);
	}
}

/* --------------------------------------------------------------------------*/



void ContextLearner::getFeatureResponse(FeaturePatch ** patches, int nr_of_patches,IplImage* rgbImage,IplImage* depthImage, IplImage* maskImage,
		int widthStep, int heightStep,FeatureResponse ***& patch_responses, FeatureResponse ***& patch_values,int *& nr_responses_saved, FeatureResponse **& list_of_values,int & nr_of_values)
{
	log(0, "Getting XYZ point cloud.");
	PointCloud* pointCloud = getXYZFromDepthImage(depthImage);
	log(0, "Labeling points belonging to object.");
	setObjectPoints(pointCloud, maskImage);

	setRGB(pointCloud, rgbImage);

	log(0, "Computing point-wise normals");
	pointCloud->fillNormals();

	pointCloud->setValidPoints();

	log(0, "Setting distance to object for each point.");
	setDistanceToObject(pointCloud);

	log(0, "Setting up Integral Image Feature");

	m_IntegralImageFeature->reset(pointCloud->m_points);
	m_numberofObjectPixels->reset(pointCloud->m_points);


	const int responses_per_file = (640*480)/(widthStep*heightStep);
	//printf("responses_per_file = %i\n",responses_per_file);
	int * responses_saved = new int[nr_of_patches];
	FeatureResponse *** patch_responses_to_save_now = new FeatureResponse**[nr_of_patches];
	FeatureResponse *** patch_values_to_save_now    = new FeatureResponse**[nr_of_patches];
	for(int i = 0; i < nr_of_patches; i++)
	{
		responses_saved[i]=0;
		patch_responses_to_save_now[i] = new FeatureResponse*[responses_per_file];
		patch_values_to_save_now[i] = new FeatureResponse*[responses_per_file];
	}

	//printf("alloc done\n");



	int maxw = pointCloud->m_width -1 -1;
	int maxh = pointCloud->m_height -1 -1;
	//cvNamedWindow("sliding box image", CV_WINDOW_AUTOSIZE);

	for (int w = 0; w <= maxw; w += widthStep){
		//printf("w: %i\n",w);
		for(int h = 0; h <= maxh; h += heightStep){
			float distance_to_center = pointCloud->m_points[w][h].z;
			if(distance_to_center!=0)
			{
				float item_size = (70.0f/0.048329)*target_object_size/max(distance_to_center,0.01f);
				float top_w = w-(0.5f*item_size);
				float top_h = h-(0.5f*item_size);
				float bot_w = w+(0.5f*item_size);
				float bot_h = h+(0.5f*item_size);
				//IplImage* rgbImageClone=cvCloneImage(depthImage);
				//Make sure we can get a value for the patches
				if(m_numberofObjectPixels->acceptable(min(max(top_w,0.0f),(float)maxw), min(max(top_h,0.0f),(float)maxh), min(max(bot_w,0.0f),(float)maxw), min(max(bot_h,0.0f),(float)maxh), 0.75f*(item_size*item_size)))
				{
					//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(255, 255, 0, 0), CV_FILLED, 8, 0);
					FeatureResponse * feature_value = m_numberofObjectPixels->getResponse(min(max(top_w,0.0f),(float)maxw), min(max(top_h,0.0f),(float)maxh), min(max(bot_w,0.0f),(float)maxw), min(max(bot_h,0.0f),(float)maxh));
					bool delete_feature_value = true;
					for(int this_patch_nr = 0; this_patch_nr < nr_of_patches; this_patch_nr++){
						FeaturePatch * this_patch = patches[this_patch_nr];
						top_w = (w+item_size*this_patch->offset_w)-(0.5f*item_size*this_patch->patch_w);
						top_h = (h+item_size*this_patch->offset_h)-(0.5f*item_size*this_patch->patch_h);
						bot_w = (w+item_size*this_patch->offset_w)+(0.5f*item_size*this_patch->patch_w);
						bot_h = (h+item_size*this_patch->offset_h)+(0.5f*item_size*this_patch->patch_h);
						if(m_IntegralImageFeature->acceptable(min(max(top_w,0.0f),(float)maxw), min(max(top_h,0.0f),(float)maxh), min(max(bot_w,0.0f),(float)maxw), min(max(bot_h,0.0f),(float)maxh), 0.75f*(item_size*item_size)*(this_patch->patch_w*this_patch->patch_h))){
							//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(0, 255, 0, 0), 1, 8, 0);
							FeatureResponse * feature_response = m_IntegralImageFeature->getResponse(min(max(top_w,0.0f),(float)maxw), min(max(top_h,0.0f),(float)maxh), min(max(bot_w,0.0f),(float)maxw), min(max(bot_h,0.0f),(float)maxh));
							delete_feature_value = false;
							patch_responses_to_save_now[this_patch_nr][responses_saved[this_patch_nr]]=feature_response;
							patch_values_to_save_now[this_patch_nr][responses_saved[this_patch_nr]]=feature_value;
							responses_saved[this_patch_nr]++;
						}
						else{
							//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(0, 0, 255, 0), 2, 8, 0);
						}
					}
					if(delete_feature_value){delete feature_value;}//If we dont find any corresponding acceptable features we should clear memory
					else{
						list_of_values[nr_of_values] = feature_value;
						nr_of_values++;
					}
				}
				else{
					//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(0, 255, 255, 0), CV_FILLED, 8, 0);
				}
				//cvShowImage("sliding box image", rgbImageClone);
				//cvWaitKey(0);
				//cvReleaseImage(&rgbImageClone);
			}
		}
	}
	//printf("internal loop done\n");
	delete pointCloud;
	patch_responses = patch_responses_to_save_now;
	patch_values = patch_values_to_save_now;
	nr_responses_saved = responses_saved;

}

/*
Extracts the base trouth and the estimated trouth by the classifiers. responses[0] contains the ground truth
 */
void ContextLearner::getClassifiersTruth(Classifier ** classifiers,int nr_classifiers,IplImage* rgbImage,IplImage* depthImage, IplImage* maskImage, int widthStep, int heightStep,float **& responses, int & nr_responses_saved)
{
	int img_width  = depthImage->width;
	int img_height = depthImage->height;
	log(0, "Getting XYZ point cloud.");
	PointCloud* pointCloud = getXYZFromDepthImage(depthImage);
	log(0, "Labeling points belonging to object.");
	setObjectPoints(pointCloud, maskImage);

	setRGB(pointCloud, rgbImage);

	log(0, "Computing point-wise normals");
	pointCloud->fillNormals();

	pointCloud->setValidPoints();

	log(0, "Setting distance to object for each point.");
	setDistanceToObject(pointCloud);

	log(0, "Setting up Integral Image Feature");
	//printf("Oo\n");

	m_IntegralImageFeature->reset(pointCloud->m_points);
	m_numberofObjectPixels->reset(pointCloud->m_points);
	int responses_per_file = (img_width*img_height) / (widthStep*heightStep);
	//nr_responses_saved = 10;
	//printf("responses per file setting upp: %i\n",responses_per_file);
	responses   = new float*[nr_classifiers+1];
	for(int i = 0; i < nr_classifiers+1; i++)
	{
		responses[i] = new float[responses_per_file];
		for(int j = 0; j < responses_per_file; j++)
		{
			responses[i][j] = 0;
		}
	}
	nr_responses_saved = 0;
	struct timeval start, end;
	long mtime, seconds, useconds; mtime = 0;
	gettimeofday(&start, NULL);
	float maxw = img_width -1 -1;
	float maxh = img_height -1 -1;

	for(int w = 0; w < img_width; w+=widthStep){
		for(int h = 0; h < img_height; h+=heightStep)
		{
			//printf("w,h = %i,%i\n",w,h);
			float distance_to_center = pointCloud->m_points[w][h].z;
			if(distance_to_center!=0)
			{
				
				float item_size = (70.0f/0.048329)*target_object_size/max(distance_to_center,0.01f);
				float top_w = w-(0.5f*item_size);
				float top_h = h-(0.5f*item_size);
				float bot_w = w+(0.5f*item_size);
				float bot_h = h+(0.5f*item_size);
				
				if(m_numberofObjectPixels->acceptable(min(max(top_w,0.0f),maxw), min(max(top_h,0.0f),maxh), min(max(bot_w,0.0f),maxw), min(max(bot_h,0.0f),maxh), 0.75f*(item_size*item_size)))
				{

					//printf("acceptable!\n");
					FeatureResponse * feature_value = m_numberofObjectPixels->getResponse(min(max(top_w,0.0f),maxw), min(max(top_h,0.0f),maxh), min(max(bot_w,0.0f),maxw), min(max(bot_h,0.0f),maxh));

					responses[0][nr_responses_saved]= feature_value->response[0];
					
					for(int i = 0; i < nr_classifiers; i++)
					{
						FeatureResponse * response = classifiers[i]->evaluate(m_IntegralImageFeature,w, h,item_size);
						if(response == NULL){
							//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(255, 255, 0, 0), CV_FILLED, 8, 0);
							float value = -1;
							responses[i+1][nr_responses_saved]=value;
						}
						else{
							float value = response->response[0];
							responses[i+1][nr_responses_saved]=value;
							delete response;
						}
					}

					nr_responses_saved++;
					delete feature_value;


				}
				else{
					//printf("unacceptable!\n");
				}
			}
		}
	}

	gettimeofday(&end, NULL);
	seconds = end.tv_sec - start.tv_sec;
	useconds = end.tv_usec - start.tv_usec;
	mtime = seconds * 1000000 + useconds;
	if (useconds < 0){
		useconds += 1000000;
		seconds--;
	}

	printf("Elapsed time total : %ld.%ld sec\n", seconds,useconds);
	delete pointCloud;

}


/* --------------------------------------------------------------------------*/

float rot_matrix[][3] = {
		{ 0.85717, 	0, 	0.51504},
		{ 0, 		1, 	0},
		{ -0.51504, 	0, 	0.85717},
};

float fi = 0.475;
void rotate_point(Point * point)
{
	float x = point->x * rot_matrix[0][0]+point->y * rot_matrix[0][1]+point->z * rot_matrix[0][2];
	float y = point->x * rot_matrix[1][0]+point->y * rot_matrix[1][1]+point->z * rot_matrix[1][2];
	float z = point->x * rot_matrix[2][0]+point->y * rot_matrix[2][1]+point->z * rot_matrix[2][2];

	point->x = x;
	point->y = y;
	point->z = z;
	//printf("rerotating\n");
}

PointCloud*
ContextLearner::getXYZFromDepthImage(IplImage* depthImage)
{

	int distance_from_cam;
	PointCloud* outCloud = new PointCloud(depthImage->width, depthImage->height);
	// FIXME: these values are possibly off
	int rgb_focal_length_VGA = 625;
	const float scale_3D_const = 0.001f / rgb_focal_length_VGA;
	const int ImageCenterX = depthImage->width >> 1;
	const int ImageCenterY = depthImage->height >> 1;

	//fi-=0.001f;

	rot_matrix[0][0] = cos(fi);
	rot_matrix[0][2] = sin(fi);
	rot_matrix[2][0] = -sin(fi);
	rot_matrix[2][2] = cos(fi);
	//printf("fi = %f -> \n",fi);
	/*for(int i = 0; i < 3; i++){
	for(int j = 0; j < 3; j++){
	printf("%5f ",rot_matrix[i][j]);
	}
	printf("\n");
}*/

	for (int i = 0; i < depthImage->width; i++)
	{
		for (int j = 0; j < depthImage->height; j++)
		{
			//FIXME : This is a really weird way of storing and getting depth value, We should just store a double value for distance in IplImage as each pixel's depth in depth map

			int part1 = 0xFF & depthImage->imageData[3 * (j * depthImage->width + i) + 0];
			int part2 = 0xFF & depthImage->imageData[3 * (j * depthImage->width + i) + 1];
			distance_from_cam = (part1 << 8) + part2;

			//Now compute X,Y,Z for each pixel
			outCloud->m_points[i][j].x = (j - ImageCenterX) * distance_from_cam * scale_3D_const;
			outCloud->m_points[i][j].y = (i - ImageCenterY) * distance_from_cam * scale_3D_const;
			outCloud->m_points[i][j].z = distance_from_cam * 0.001; // because values are in mm

			Point * this_point = &(outCloud->m_points[i][j]);
			rotate_point(this_point);

		}
	}

	//outCloud->filterXYZ();//Lms filtering



	return outCloud;
}

/* --------------------------------------------------------------------------*/
void
ContextLearner::setDistanceToObject(PointCloud* pointCloud)
{

	bool ** tested = new bool *[640];
	for (int i = 0; i < 640; i++)
	{
		tested[i] = new bool[480];
		for (int j = 0; j < 480; j++)
		{
			tested[i][j] = false;
		}
	}

	Point *** components = new Point**[15];
	int * points_in_component = new int[15];
	int nr_components = 0;
	int nr_in_list = 0;
	int * int_x = new int[1000000];
	int * int_y = new int[1000000];
	//TODO: USE LISTS! Fix memleeks

	//Finds connected components
	for (int i = 0; i < 640; i++)
	{
		for (int j = 0; j < 480; j++)
		{
			if (!tested[i][j])
			{
				if ((*pointCloud)(i, j).is_object && (*pointCloud)(i, j).is_valid)
				{

					components[nr_components] = new Point*[1000000];

					nr_in_list = 1;
					int k = 0;
					int_x[k] = i;
					int_y[k] = j;
					components[nr_components][k] = &(*pointCloud)(int_x[k], int_y[k]);
					tested[int_x[k]][int_y[k]] = true;

					while (k < nr_in_list)
					{
						int tmpx = int_x[k] + 1;
						int tmpy = int_y[k];
						if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
						{
							if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
							{
								components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy] = true;
								nr_in_list++;
							}
						}
						tmpx = int_x[k] - 1;
						tmpy = int_y[k];
						if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
						{
							if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
							{
								components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy] = true;
								nr_in_list++;
							}
						}
						tmpx = int_x[k];
						tmpy = int_y[k] + 1;
						if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
						{
							if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
							{
								components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy] = true;
								nr_in_list++;
							}
						}
						tmpx = int_x[k];
						tmpy = int_y[k] - 1;
						if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
						{
							if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
							{
								components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy] = true;
								nr_in_list++;
							}
						}
						k++;
					}
					points_in_component[nr_components] = nr_in_list;
					nr_components++;
				}
				tested[i][j] = true;
			}
		}
	}

	float * median_x = new float[nr_components];
	float * median_y = new float[nr_components];
	float * median_z = new float[nr_components];
	for (int i = 0; i < nr_components; i++)
	{
		float * medx = new float[points_in_component[i]];
		float * medy = new float[points_in_component[i]];
		float * medz = new float[points_in_component[i]];

		for (int j = 0; j < points_in_component[i]; j++)
		{
			medx[j] = components[i][j]->x;
			medy[j] = components[i][j]->y;
			medz[j] = components[i][j]->z;
		}

		median_x[i] = quick_select(medx, points_in_component[i]);
		median_y[i] = quick_select(medy, points_in_component[i]);
		median_z[i] = quick_select(medz, points_in_component[i]);
		/*printf("------------------------------------------------------------------\n");
	printf("------------------------------------------------------------------\n");
	printf("------------------------------------------------------------------\n");
	printf("------------------------------------------------------------------\n");
	printf("------------------------------------------------------------------\n");
	printf("------------------------------------------------------------------\n");
	printf("------------------------------------------------------------------\n");*/
		float * meddist = new float[points_in_component[i]];
		for (int j = 0; j < points_in_component[i]; j++)
		{
			meddist[j] = sqrt(pow(medx[j]-median_x[i],2)+pow(medy[j]-median_y[i],2)+pow(medz[j]-median_z[i],2));
			//printf("%f = sqrt((%f-%f)^2 + (%f-%f)^2 + (%f-%f)^2)\n",meddist[j],medx[j],median_x[i],medy[j],median_y[i],medz[j],median_z[i]);
		}
		float size_object = quick_select(meddist, points_in_component[i]);
		//printf("object size: %f\n",size_object);
		delete medx;
		delete medy;
		delete medz;
		//printf("center = [%f %f %f]\n",median_x[i],median_y[i],median_z[i]);
	}

	for (int i = 0; i < 640; i++)
	{
		for (int j = 0; j < 480; j++)
		{
			for (int k = 0; k < nr_components; k++)
			{
				// FIXME initialize pointer to pointCloud member once and use that instead of calling () function operator all the time
				float xsquare = ((*pointCloud)(i, j).x - median_x[k]) * ((*pointCloud)(i, j).x - median_x[k]);
				float ysquare = ((*pointCloud)(i, j).y - median_y[k]) * ((*pointCloud)(i, j).y - median_y[k]);
				float zsquare = ((*pointCloud)(i, j).z - median_z[k]) * ((*pointCloud)(i, j).z - median_z[k]);
				float dist = xsquare + ysquare + zsquare;
				if (dist < (*pointCloud)(i, j).distance_to_object)
				{
					(*pointCloud)(i, j).distance_to_object = dist;
				}
			}
		}
	}

	delete median_x;
	delete median_y;
	delete median_z;

	for(int i = 0; i < nr_components; i++){delete[] components[i];}
	delete[] components;
	delete[] points_in_component;
	delete[] int_x;
	delete[] int_y;
	for (int i = 0; i < 640; i++){delete[] tested[i];}
	delete[] tested;
}

/* --------------------------------------------------------------------------*/
void
ContextLearner::setObjectPoints(PointCloud* pointcloud, IplImage* mask_image)
{
	bool is_object = false;
	for (int i = 0; i < pointcloud->m_width; i++)
	{
		for (int j = 0; j < pointcloud->m_height; j++)
		{
			is_object = (0xFF & (mask_image->imageData[3 * (j * pointcloud->m_width + i) + 0])) > 0;
			pointcloud->m_points[i][j].is_object = is_object;
		}
	}
}

/* --------------------------------------------------------------------------*/

void
ContextLearner::setRGB(PointCloud* pointcloud, IplImage* rgb_image)
{
	bool is_object = false;
	for (int i = 0; i < pointcloud->m_width; i++)
	{
		for (int j = 0; j < pointcloud->m_height; j++)
		{
			pointcloud->m_points[i][j].b = (float)(0xFF & (rgb_image->imageData[3 * (j * pointcloud->m_width + i) + 0]));
			pointcloud->m_points[i][j].g = (float)(0xFF & (rgb_image->imageData[3 * (j * pointcloud->m_width + i) + 1]));
			pointcloud->m_points[i][j].r = (float)(0xFF & (rgb_image->imageData[3 * (j * pointcloud->m_width + i) + 2]));
			pointcloud->m_points[i][j].b /= 256.0f;
			pointcloud->m_points[i][j].g /= 256.0f;
			pointcloud->m_points[i][j].r /= 256.0f;
			//printf("123 ->r = %f, g = %f, b = %f\n",pointcloud->m_points[i][j].r,pointcloud->m_points[i][j].g,pointcloud->m_points[i][j].b);
			//exit(0);
		}
	}
}

/* --------------------------------------------------------------------------*/

bool
ContextLearner::readNextImageSet(IplImage * &outRGBImage, IplImage * &outDepthImage, IplImage * &outMaskImage)
{
	if (m_nextImageIndex >= m_trainingImagePaths.size())
	{
		log(3, "Reached at the end of training image list; read %d images", m_nextImageIndex);
		return false;
	}
	//printf("m_nextImageIndex = %i\n",m_nextImageIndex);
	//printf("%s \n",m_trainingImagePaths[m_nextImageIndex].filePathRGB.c_str());
	outRGBImage = cvLoadImage(m_trainingImagePaths[m_nextImageIndex].filePathRGB.c_str(), CV_LOAD_IMAGE_COLOR);
	outDepthImage = cvLoadImage(m_trainingImagePaths[m_nextImageIndex].filePathDepth.c_str(), CV_LOAD_IMAGE_COLOR);
	outMaskImage = cvLoadImage(m_trainingImagePaths[m_nextImageIndex].filePathMask.c_str(), CV_LOAD_IMAGE_COLOR);
	m_nextImageIndex+=1;//<-----------------------just for testing... set to 1 otherwise

	return true;
}

float ContextLearner::estimate_size_of_object(){
	IplImage * image_rgb;
	IplImage * image_depth;
	IplImage * image_mask;
	m_nextImageIndex = 0;
	float * all_object_sizes = new float[10000];
	int objects_so_far = 0;
	while (readNextImageSet(image_rgb, image_depth, image_mask))
	{
		//printf("m_nextImageIndex = %i\n",m_nextImageIndex);
		PointCloud* pointCloud = getXYZFromDepthImage(image_depth);
		setObjectPoints(pointCloud, image_mask);
		pointCloud->setValidPoints();

		bool ** tested = new bool *[640];
		for (int i = 0; i < 640; i++)
		{
			tested[i] = new bool[480];
			for (int j = 0; j < 480; j++)
			{
				tested[i][j] = false;
			}
		}
		Point *** components = new Point**[15];
		int * points_in_component = new int[15];
		int nr_components = 0;
		int nr_in_list = 0;
		int * int_x = new int[1000000];
		int * int_y = new int[1000000];
		//TODO: USE LISTS!

		//Finds connected components
		for (int i = 0; i < 640; i++)
		{
			for (int j = 0; j < 480; j++)
			{
				if (!tested[i][j])
				{
					if ((*pointCloud)(i, j).is_object && (*pointCloud)(i, j).is_valid)
					{
						components[nr_components] = new Point*[1000000];
						nr_in_list = 1;
						int k = 0;
						int_x[k] = i;
						int_y[k] = j;
						components[nr_components][k] = &(*pointCloud)(int_x[k], int_y[k]);
						tested[int_x[k]][int_y[k]] = true;
						while (k < nr_in_list)
						{
							int tmpx = int_x[k] + 1;
							int tmpy = int_y[k];
							if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
							{
								if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
								{
									components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
									int_x[nr_in_list] = tmpx;
									int_y[nr_in_list] = tmpy;
									tested[tmpx][tmpy] = true;
									nr_in_list++;
								}
							}
							tmpx = int_x[k] - 1;
							tmpy = int_y[k];
							if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
							{
								if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
								{
									components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
									int_x[nr_in_list] = tmpx;
									int_y[nr_in_list] = tmpy;
									tested[tmpx][tmpy] = true;
									nr_in_list++;
								}
							}
							tmpx = int_x[k];
							tmpy = int_y[k] + 1;
							if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
							{
								if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
								{
									components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
									int_x[nr_in_list] = tmpx;
									int_y[nr_in_list] = tmpy;
									tested[tmpx][tmpy] = true;
									nr_in_list++;
								}
							}
							tmpx = int_x[k];
							tmpy = int_y[k] - 1;
							if (tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0)
							{
								if (!tested[tmpx][tmpy] && (*pointCloud)(tmpx, tmpy).is_object && (*pointCloud)(tmpx, tmpy).is_valid)
								{
									components[nr_components][nr_in_list] = &(*pointCloud)(tmpx, tmpy);
									int_x[nr_in_list] = tmpx;
									int_y[nr_in_list] = tmpy;
									tested[tmpx][tmpy] = true;
									nr_in_list++;
								}
							}
							k++;
						}
						points_in_component[nr_components] = nr_in_list;
						nr_components++;
					}
					tested[i][j] = true;
				}
			}
		}

		float * median_x = new float[nr_components];
		float * median_y = new float[nr_components];
		float * median_z = new float[nr_components];
		for (int i = 0; i < nr_components; i++)
		{
			float * medx = new float[points_in_component[i]];
			float * medy = new float[points_in_component[i]];
			float * medz = new float[points_in_component[i]];

			for (int j = 0; j < points_in_component[i]; j++)
			{
				medx[j] = components[i][j]->x;
				medy[j] = components[i][j]->y;
				medz[j] = components[i][j]->z;
			}

			median_x[i] = quick_select(medx, points_in_component[i]);
			median_y[i] = quick_select(medy, points_in_component[i]);
			median_z[i] = quick_select(medz, points_in_component[i]);

			float * meddist = new float[points_in_component[i]];
			for (int j = 0; j < points_in_component[i]; j++)
			{
				meddist[j] = sqrt(pow(medx[j]-median_x[i],2)+pow(medy[j]-median_y[i],2)+pow(medz[j]-median_z[i],2));
			}
			float size_object = quick_select(meddist, points_in_component[i]);
			all_object_sizes[objects_so_far] = size_object;
			objects_so_far++;
			delete[] medx;
			delete[] medy;
			delete[] medz;
		}
		delete[] median_x;
		delete[] median_y;
		delete[] median_z;

		for(int i = 0; i < nr_components; i++){delete[] components[i];}
		delete[] components;
		delete[] points_in_component;
		delete[] int_x;
		delete[] int_y;
		for (int i = 0; i < 640; i++){delete[] tested[i];}
		delete[] tested;

		delete pointCloud;
		cvReleaseImage(&image_rgb);
		cvReleaseImage(&image_depth);
		cvReleaseImage(&image_mask);
	}
	float median_all_object_sizes = quick_select(all_object_sizes, objects_so_far);
	printf("median_all_object_sizes: %f\n",median_all_object_sizes);
	delete[] all_object_sizes;
	m_nextImageIndex = 0;
	return median_all_object_sizes;
}

float ContextLearner::evaluate_regressor(Classifier ** classifiers,int nr_classifiers, IntegralImageFeature * iifeature, int widthStep, int heightStep){
	//exit(0);
	IplImage * image_rgb;
	IplImage * image_depth;
	IplImage * image_mask;
	float * total_error = new float[nr_classifiers];
	float * total_win = new float[nr_classifiers];
	float * total_imgs = new float[nr_classifiers];
	float * total_time  = new float[nr_classifiers];
	for(int i = 0; i < nr_classifiers; i++){total_error[i] = 0;total_time[i] = 0;total_win[i] = 0;total_imgs[i] = 0;}

	//target_object_size = estimate_size_of_object();
	m_nextImageIndex = 0;
	int features_stored = 0;
	Classifier * echodata = new EchoClassifier();
	echodata->patch = new FeaturePatch();
	echodata->patch->offset_w  = 0;
	echodata->patch->offset_h  = 0;
	echodata->patch->patch_w   = 1;
	echodata->patch->patch_h   = 1;

	while (readNextImageSet(image_rgb, image_depth, image_mask))
	{
		char buf[256];


		int img_width  = image_depth->width;
		int img_height = image_depth->height;
		float ** mat = new float*[img_width];
		float ** mat_div = new float*[img_width];
		for (int i = 0; i < img_width; i++){
			mat[i] = new float[img_height];
			mat_div[i] = new float[img_height];
			for (int j = 0; j < img_height; j++){
				mat_div[i][j] = 0;
				mat[i][j] = 0;
			}
		}


		PointCloud* pointCloud = getXYZFromDepthImage(image_depth);
		setObjectPoints(pointCloud, image_mask);
		setRGB(pointCloud, image_rgb);
		pointCloud->fillNormals();
		pointCloud->setValidPoints();
		setDistanceToObject(pointCloud);

		m_numberofObjectPixels->reset(pointCloud->m_points);
		m_numberofObjectPixels->interpolate_method = 1;



		for(int w = 0; w < img_width; w++){
			for(int h = 0; h < img_height; h++){
				float distance_to_center = pointCloud->m_points[w][h].z;
				if(distance_to_center!=0)
				{
					float item_size = (70.0f/0.048329)*target_object_size/max(distance_to_center,0.01f);
					float top_w = w-(0.5f*item_size);
					float top_h = h-(0.5f*item_size);
					float bot_w = w+(0.5f*item_size);
					float bot_h = h+(0.5f*item_size);
					FeatureResponse * response = echodata->evaluate(m_numberofObjectPixels,w, h,item_size);
					if(response != NULL){
						//printf("DID NOT GET null..\n");
						top_w = min(max(top_w,0.0f),(float)img_width-1);
						top_h = min(max(top_h,0.0f),(float)img_height-1);
						bot_w = min(max(bot_w,0.0f),(float)img_width-1);
						bot_h = min(max(bot_h,0.0f),(float)img_height-1);
						float value = response->response[0];
						if (!isnan(value))
						{
							for (int i = top_w; i < bot_w; i++)
							{
								for (int j = top_h; j < bot_h; j++)
								{
									mat[i][j] += value;
									mat_div[i][j]++;
								}
							}
						}
						delete response;
					}
					else{
						//printf("got null..\n");
					}
				}
				else{
					//printf("[%i,%i] distance_to_center = 0\n",w,h);
				}
			}
		}

		bool valid_test_image = false;
		float base_largest = 0;
		for (int i = 0; i < img_width; i++){
			for (int j = 0; j < img_height; j++){
				if(mat_div[i][j] > 0){
					mat[i][j] /= mat_div[i][j];
					if(mat[i][j] > base_largest){
						base_largest = mat[i][j];
					}
				}
				else{
					mat[i][j] = 0;
				}
			}
		}

		if(base_largest > 0){
			for (int i = 0; i < img_width; i++){
				for (int j = 0; j < img_height; j++){
					mat[i][j] /= base_largest;
				}
			}
			valid_test_image = true;
		}
		//printf("base_largest = %f\n",base_largest);

		sprintf(buf,"outputimages/%i_base.bmp",m_nextImageIndex);
		//printf("Saving to file:%s \n",buf);
		save_matrix(buf,mat, 640, 480);

if(valid_test_image)
{
		//printf("evaluting %i\n",m_nextImageIndex);
		iifeature->reset(pointCloud->m_points);
		iifeature->interpolate_method = 0;
		for(int cl_number = 0; cl_number < nr_classifiers; cl_number++)
		{
			//printf("cl:%i\n",cl_number);


			float ** out_mat = new float*[img_width];
			float ** out_mat_div = new float*[img_width];

			for (int i = 0; i < img_width; i++){
				out_mat[i] = new float[img_height];
				out_mat_div[i] = new float[img_height];
				for (int j = 0; j < img_height; j++){
					out_mat_div[i][j] = 0;
					out_mat[i][j] = 0;
				}
			}


			struct timeval start, end;
			long mtime, seconds, useconds; mtime = 0;




			//cvNamedWindow("Eval sliding box image", CV_WINDOW_AUTOSIZE);
			
			for(int w = 0; w < img_width; w+=widthStep)
			{
				for(int h = 0; h < img_height; h+=heightStep)
				{
					float distance_to_center = pointCloud->m_points[w][h].z;
					if(distance_to_center!=0)
					{
						gettimeofday(&start, NULL);
						float item_size = (70.0f/0.048329)*target_object_size/max(distance_to_center,0.01f);
						float top_w = w-(0.5f*item_size);
						float top_h = h-(0.5f*item_size);
						float bot_w = w+(0.5f*item_size);
						float bot_h = h+(0.5f*item_size);
						//IplImage* rgbImageClone=cvCloneImage(depthImage);
						FeatureResponse * response = classifiers[cl_number]->evaluate(iifeature,w, h,item_size);
						if(response == NULL){
							//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(255, 255, 0, 0), CV_FILLED, 8, 0);
							gettimeofday(&end, NULL);
							seconds = end.tv_sec - start.tv_sec;
							useconds = end.tv_usec - start.tv_usec;
							mtime += seconds * 1000000 + useconds;
							if (useconds < 0){
								useconds += 1000000;
								seconds--;
							}
						}
						else{
							//FeaturePatch * this_patch = cl->patches[0];
							top_w = min(max(top_w,0.0f),(float)img_width-1);
							top_h = min(max(top_h,0.0f),(float)img_height-1);
							bot_w = min(max(bot_w,0.0f),(float)img_width-1);
							bot_h = min(max(bot_h,0.0f),(float)img_height-1);
							float value = response->response[0];



							gettimeofday(&end, NULL);
							seconds = end.tv_sec - start.tv_sec;
							useconds = end.tv_usec - start.tv_usec;
							mtime += seconds * 1000000 + useconds;
							if (useconds < 0){
								useconds += 1000000;
								seconds--;
							}

							if (!isnan(value))
							{
								for (int i = top_w; i < bot_w; i++)
								{
									for (int j = top_h; j < bot_h; j++)
									{
										out_mat[i][j] += value;
										out_mat_div[i][j]++;
									}
								}
							}
							delete response;
							//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(value*255, value*255, value*255, 0), CV_FILLED, 8, 0);
							//cvRectangle(rgbImageClone, cvPoint(round(tmp_top_w), round(tmp_top_h)), cvPoint(round(tmp_bot_w), round(tmp_bot_h)),cvScalar(255, 0, 255, 0), 2, 8, 0);
						}
		
						//cvRectangle(rgbImageClone, cvPoint(round(top_w), round(top_h)), cvPoint(round(bot_w), round(bot_h)),cvScalar(255, 255, 0, 0), CV_FILLED, 8, 0);
						//cvShowImage("Eval sliding box image", rgbImageClone);
						//cvWaitKey(0);
						//cvReleaseImage(&rgbImageClone);
					}
				}
			}

			float largest = 0;
			//printf("Elapsed time total : %f sec\n", ((float)mtime)/1000000.0f);
			float total_obj = 0;
			float total_max = 0;

			for (int i = 0; i < img_width; i++){
				for (int j = 0; j < img_height; j++){
					if(out_mat_div[i][j] > 0){
						out_mat[i][j] /=out_mat_div[i][j];
						if(out_mat[i][j] > largest){
							largest = out_mat[i][j];
							total_max = 1;
							total_obj = 0;
							if(pointCloud->m_points[i][j].is_object){total_obj++;}
						}else if(out_mat[i][j] == largest)
						{
							total_max++;
							if(pointCloud->m_points[i][j].is_object){total_obj++;}
						}
					}
					else{
						out_mat[i][j] = 0;
					}
				}
			}

			total_imgs[cl_number]++;
			total_win[cl_number] += total_obj/total_max;

			float error = 0;
			for (int i = 0; i < img_width; i++){
				for (int j = 0; j < img_height; j++){
					if(largest > 0){out_mat[i][j]/=largest;}
					if(out_mat_div[i][j] > 0){
						//printf("error[%i][%i] -> %f  %f\n",i,j,mat[i][j],out_mat[i][j]);
						error += (mat[i][j]-out_mat[i][j])*(mat[i][j]-out_mat[i][j]);
					}
				}
			}
			total_error[cl_number] += error;
			total_time[cl_number]  += mtime;
			//printf("total error = %f\n",total_error);

		sprintf(buf,"outputimages/%i_reg%i_out_mat.bmp",m_nextImageIndex,cl_number+1);
		printf("Saving to file:%s \n",buf);
		save_matrix(buf,out_mat, 640, 480);

			for (int i = 0; i < img_width; i++){
				delete[] out_mat_div[i];
				delete[] out_mat[i];
			}
			delete[] out_mat_div;
			delete[] out_mat;

		}
}

		/*printf("Er=[%8i ",(int)total_error[0]);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8i", (int)total_error[i]);
		}
		printf("]\n");

		printf("Ti=[%8.3f ",total_time[0]/1000000);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8.3f", total_time[i]/1000000);
		}
		printf("]\n");

		printf("percentages=[%8.3f ",total_win[0]);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8.3f", total_win[i]);
		}
		printf("]\n");

		printf("Images=[%8.3f ",total_imgs[0]);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8.3f", total_imgs[i]);
		}
		printf("]\n");*/

		for(int i = 0; i < img_width; i++){
			delete[] mat_div[i];
			delete[] mat[i];
		}
		delete[] mat_div;
		delete[] mat;
		delete pointCloud;
		cvReleaseImage(&image_rgb);
		cvReleaseImage(&image_depth);
		cvReleaseImage(&image_mask);
	}
		printf("Er=[%8i ",(int)total_error[0]);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8i", (int)total_error[i]);
		}
		printf("]\n");

		printf("Ti=[%8.3f ",total_time[0]/1000000);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8.3f", total_time[i]/1000000);
		}
		printf("]\n");

		printf("percentages=[%8.3f ",total_win[0]);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8.3f", total_win[i]);
		}
		printf("]\n");

		printf("Images=[%8.3f ",total_imgs[0]);
		for(int i = 1; i < nr_classifiers; i++){
			printf(", %8.3f", total_imgs[i]);
		}
		printf("]\n");
	delete[] total_time;
	delete[] total_error;
	return 0;//total_error;
}




/* --------------------------------------------------------------------------*/
void
ContextLearner::train()
{
	char * classifier_names = (char *)"./output/svm_classifier_v11";
	char * responses_path   = (char *)"./output/responses_v11_1_naive";
	char * boost_reg_path   = (char *)"./output/boost_final_v5_2_boostedVII";
	//boost_reg_path   = (char *)"./output/boost_final_v5_2_boostedVII";

//	BoostedClassifierVII * boosted_classifier_new = new BoostedClassifierVII(classifier_names,responses_path,boost_reg_path,20);
//exit(0);

	int test_max_number_features = 200000;
	int f_per_img = test_max_number_features/m_trainingImagePaths.size();
	float step_size_test = sqrt((float)((640*480)/f_per_img));
	printf("sugested step: %f\n",step_size_test);
	int step = step_size_test;
	if(step == 0){step = 1;}
	int max_number_features = (640*480*m_trainingImagePaths.size())/(step*step);
	target_object_size = estimate_size_of_object();

	printf("=====================================================================================\n");
	printf("=====================================================================================\n");
	printf("==========================[target_object_size = %f]==============================\n",target_object_size);
	printf("======================================123===============================================\n");
	printf("=====================================================================================\n");

	
	
	Classifier ** boosted_classifier = new Classifier*[10000];

	for(int i = 1; i <= 20; i++)
	{
		char filename[256];
		sprintf(filename, "%s_%i",boost_reg_path, i);
		printf("loading %s\n",filename);
		boosted_classifier[i-1] = new BoostedClassifierVII(filename);
	}
	//for(int i = 12; i > 1; i-=4)
	//{
	//	printf("123step = %i\n",i);
		evaluate_regressor(boosted_classifier,20, m_IntegralImageFeature, 1, 1);
	//}
exit(0);

	
	int current_regressor = 0;
	float step_of= 0.20f;//stepwidth in offset
	float max_of = step_of*6;//Max offset distance
	float max_p  = 1.5f;//max patchsize
	float step_p = 0.5f;//stepwidth in patchsize
	FeaturePatch ** all_patches = new FeaturePatch*[10000];//FIX: clear mem after done with these
	int all_nr_of_patches = 0;
	for(float p_w = step_p; p_w <= max_p; p_w += step_p){
		for(float p_h = step_p; p_h <= max_p; p_h += step_p){
			for(float of_w = -max_of; of_w <= max_of; of_w += step_of){
				for(float of_h = -max_of; of_h <= max_of; of_h += step_of){
					all_patches[all_nr_of_patches] = new FeaturePatch();
					all_patches[all_nr_of_patches]->offset_w  = of_w;
					all_patches[all_nr_of_patches]->offset_h  = of_h;
					all_patches[all_nr_of_patches]->patch_w   = p_w;
					all_patches[all_nr_of_patches]->patch_h   = p_h;
					all_nr_of_patches++;
				}
			}
		}
	}
	printf("all_nr_of_patches = %i\n",all_nr_of_patches);


	int nr_of_patches = 100;
	int tmp_nr_of_patches = nr_of_patches;
	int * patches_responses_saved = new int[nr_of_patches];
	FeatureResponse*** patches_allImageFeatureResponses = new FeatureResponse**[nr_of_patches];
	FeatureResponse*** patches_allImageFeatureValues = new FeatureResponse**[nr_of_patches];
	FeatureResponse ** list_of_values = new FeatureResponse*[max_number_features];
	for(int i = 0; i < nr_of_patches; i++)
	{
		patches_allImageFeatureResponses[i] = new FeatureResponse*[max_number_features];
		patches_allImageFeatureValues[i] = new FeatureResponse*[max_number_features];
		patches_responses_saved[i] = 0;
	}

	for(int patch_counter = 0; patch_counter * nr_of_patches < all_nr_of_patches;patch_counter += 1)
	{
		tmp_nr_of_patches = nr_of_patches;
		if((nr_of_patches * patch_counter) + nr_of_patches > all_nr_of_patches)
		{
			printf("too larges... nr_of_patches should be %i\n",all_nr_of_patches-(nr_of_patches * patch_counter));
			tmp_nr_of_patches = all_nr_of_patches-(nr_of_patches * patch_counter);
		}
		printf("patch_counter = %i\n",patch_counter);
		printf("nr_of_patches * patch_counter = %i\n",nr_of_patches * patch_counter);
		FeaturePatch ** patches = new FeaturePatch*[nr_of_patches];
		for(int tmp_patch_id = 0; tmp_patch_id < tmp_nr_of_patches; tmp_patch_id++)
		{
			patches[tmp_patch_id] = all_patches[nr_of_patches * patch_counter + tmp_patch_id];
			printf("Patches to investigate contains: %i: %i\n",tmp_patch_id,nr_of_patches * patch_counter + tmp_patch_id,patches[tmp_patch_id]);
		}
		int nr_of_values = 0;

		IplImage * image_rgb;
		IplImage * image_depth;
		IplImage * image_mask;

		m_nextImageIndex = 0;
		int features_stored = 0;
		printf("Pre loop\n");
		while (readNextImageSet(image_rgb, image_depth, image_mask))
		{
			printf("Loop\n");
			FeatureResponse *** patch_responses;
			FeatureResponse *** patch_values;
			int * nr_responses_saved;
			getFeatureResponse(patches,tmp_nr_of_patches,image_rgb,image_depth, image_mask, step , step , patch_responses,  patch_values, nr_responses_saved, list_of_values,nr_of_values);
			for(int i = 0; i < tmp_nr_of_patches; i++)
			{
				for(int j = 0; j < nr_responses_saved[i]; j++)
				{
					patches_allImageFeatureResponses[i][patches_responses_saved[i]] = patch_responses[i][j];
					patches_allImageFeatureValues[i][patches_responses_saved[i]] = patch_values[i][j];
					patches_responses_saved[i]++;
				}
			}
			for(int i = 0; i < tmp_nr_of_patches; i++)
			{
				delete patch_responses[i];
				delete patch_values[i];
			}
			delete patch_responses;
			delete patch_values;
			delete nr_responses_saved;

			cvReleaseImage(&image_rgb);
			cvReleaseImage(&image_depth);
			cvReleaseImage(&image_mask);
		}

		int max_cluster_size = 40;
		char filename[256];
		for(int nr_feat_for_train = 40; nr_feat_for_train <= max_cluster_size; nr_feat_for_train =(1.6f*((float)nr_feat_for_train))+2)
		{
			printf("============================================\n============================================\nClusters: %i\n============================================\n============================================\n",nr_feat_for_train);
			for(int i = 0; i < tmp_nr_of_patches; i++)
			{
				FeatureResponse ** out_values;
				FeatureResponse ** out_responses;
				float ** data_weights = new float*;
				struct timeval start, end;
				gettimeofday(&start, NULL);
				try
				{
					printf("starting clustering: %i with %i clusters\n",i,nr_feat_for_train);
					cluster(patches_allImageFeatureValues[i], patches_allImageFeatureResponses[i], out_values, out_responses, data_weights, patches_responses_saved[i],nr_feat_for_train);
				}
				catch (const char* c)
				{
					printf("Caught exception: %s \n", c);
					exit(0);
				}

				SVMClassifier * tmpclassifier = new SVMClassifier;
				tmpclassifier -> patch = patches[i];
				tmpclassifier ->validation_target = patches_allImageFeatureValues[i];
				tmpclassifier ->validation_data = patches_allImageFeatureResponses[i];
				tmpclassifier ->validation_nr_data = patches_responses_saved[i];
				tmpclassifier ->train(out_values, out_responses, *data_weights, nr_feat_for_train);
				sprintf(filename, "%s_%i",classifier_names ,current_regressor);
				printf("saving %s\n",filename);
				tmpclassifier ->save(filename);
				current_regressor++;
				delete tmpclassifier;

				for(int j = 0; j < nr_feat_for_train; j++){
					delete out_values[j];
					delete out_responses[j];
				}
				delete[] out_values;
				delete[] out_responses;
				delete[] data_weights;
				gettimeofday(&end, NULL);
				printf("Elapsed time training total : %f sec\n", (float) (((double) ((end.tv_sec - start.tv_sec) * 1000000 + (end.tv_usec - start.tv_usec))) / ((double) 1000000)));

			}
		}

		printf("clearing mem\n");
		for(int i = 0; i < nr_of_values; i++)
		{
			delete list_of_values[i];
		}

		for(int i = 0; i < tmp_nr_of_patches; i++)
		{
			for(int j = 0; j < patches_responses_saved[i]; j++)
			{
				delete patches_allImageFeatureResponses[i][j];
			}
			patches_responses_saved[i] = 0;
		}
		printf("done clear mem\n");
	}

	delete list_of_values;
	for(int i = 0; i < tmp_nr_of_patches; i++)
	{
		delete patches_allImageFeatureResponses[i];
		delete patches_allImageFeatureValues[i];
	}
	delete patches_responses_saved;
	delete patches_allImageFeatureResponses;
	delete patches_allImageFeatureValues;











	bool loop = true;
	Classifier ** list_of_classifiers = new Classifier*[100000];
	int nr_of_regressors = 0;
	for(int i = 0; loop ; i++)
	{
		Classifier * classifier = new SVMClassifier();
		char filename[256];
		sprintf(filename, "%s_%i",classifier_names, i);
		loop = classifier->load(filename);
		if(loop)
		{
			list_of_classifiers[i] = classifier;
			nr_of_regressors = i+1;
			printf("=========%i=========\n",nr_of_regressors);
		}
		else{printf("loaded %i regressors\n",nr_of_regressors);}
	}






int nr_of_regressors_now = 150;

float ** responses = new float*[nr_of_regressors_now+1];
for(int i = 0; i < nr_of_regressors_now+1; i++){
	responses[i] = new float[max_number_features];
	for(int j = 0; j < max_number_features; j++)
	{
		responses[i][j] = 0;
	}
}
printf("max_number_features = %i\n",max_number_features);


for(int regressor_subset_start = 0; regressor_subset_start < nr_of_regressors; regressor_subset_start += nr_of_regressors_now)
{
	if(regressor_subset_start + nr_of_regressors_now >= nr_of_regressors)
	{
		nr_of_regressors_now = nr_of_regressors - regressor_subset_start;
		printf("nr_of_regressors_now = %i\n",nr_of_regressors_now);
	}
	printf("regressor_subset_start = %i\n",regressor_subset_start);
	Classifier ** list_of_classifiers_now = new Classifier*[nr_of_regressors_now];
	for(int i = 0; i < nr_of_regressors_now; i++)
	{
		list_of_classifiers_now[i] = list_of_classifiers[regressor_subset_start+i];
		//printf("%i:%i\n",i,regressor_subset_start+i);
	}
	int classifier_responses_saved = 0;

	IplImage * image_rgb;
	IplImage * image_depth;
	IplImage * image_mask;



	m_nextImageIndex = 0;
	printf("Before while loop!\n");
	while (readNextImageSet(image_rgb, image_depth, image_mask))
	{
		int tmp_responses_saved = 0;
		float ** tmp_responses;
		getClassifiersTruth(list_of_classifiers_now,nr_of_regressors_now,image_rgb,image_depth, image_mask, step, step,tmp_responses, tmp_responses_saved);		
		for(int j = 0; j < tmp_responses_saved; j++)
		{
			for(int i = 0; i < nr_of_regressors_now+1; i++)
			{
				responses[i][classifier_responses_saved] = tmp_responses[i][j];
			}
			classifier_responses_saved++;
		}

		for(int i = 0; i < nr_of_regressors_now+1; i++){delete[] tmp_responses[i];}
		delete[] tmp_responses;
		
		
		cvReleaseImage(&image_rgb);
		cvReleaseImage(&image_depth);
		cvReleaseImage(&image_mask);
	}

printf("responses saved: %i\n",classifier_responses_saved);

	for(int i = 0; i < nr_of_regressors_now+1;i++)
	{
		if(i == 0){
			char filename_bin[256];
			sprintf(filename_bin, "%s_%i.results",responses_path,0);
			printf("Storing answers: %s\n",filename_bin);
			ofstream outfile_bin (filename_bin,ofstream::binary);
			outfile_bin.write ((char *)(responses[0]),classifier_responses_saved*4);
			outfile_bin.close();
		}else{
			char filename_bin[256];
			sprintf(filename_bin, "%s_%i.results",responses_path,i+regressor_subset_start);
			printf("Storing estimates: %s\n",filename_bin);
			ofstream outfile_bin (filename_bin,ofstream::binary);
			outfile_bin.write ((char *)(responses[i]),classifier_responses_saved*4);
			outfile_bin.close();
		}
	}

	delete[] list_of_classifiers_now;
	

	char filename_base[256];
	sprintf(filename_base, "%s.base",responses_path);
	int output_base_size = 3;

	float * output_base = new float[output_base_size];
	output_base[0] = classifier_responses_saved;
	output_base[1] = nr_of_regressors;
	output_base[2] = target_object_size;
	ofstream outfile_base (filename_base,ofstream::binary);
	outfile_base.write ((char *)output_base,output_base_size*4);
	outfile_base.close();


	classifier_responses_saved = 0;
}


float * total_for_improvement   = new float[max_number_features];
float * ammount_for_improvement = new float[max_number_features];
float * tmp_improved 	    = new float[max_number_features];
for(int i = 0; i < max_number_features; i++)
{
	total_for_improvement[i] = 0;
	ammount_for_improvement[i] = 0;
}

bool loop_read = true;
int index = 1;
int nr_data = 0;
int nr_regs = 0;	
while(loop_read)
{
	char filename_bin[256];
	sprintf(filename_bin, "%s_%i.results",responses_path, index);
	ifstream::pos_type fsize_bin;
	ifstream file_bin (filename_bin, ios::in|ios::binary|ios::ate);
	if (file_bin.is_open())
	{
		printf("Opening: %s\n",filename_bin);
		fsize_bin = file_bin.tellg();
		file_bin.seekg (0, ios::beg);
		file_bin.read ((char *)tmp_improved, fsize_bin);
		file_bin.close();
		nr_data = fsize_bin/4;
		for(int i = 0; i < nr_data; i++)
		{
			if(tmp_improved[i]==-1){}
			else{
				total_for_improvement[i] =+tmp_improved[i];
				ammount_for_improvement[i]++;
			}
		}
		nr_regs++;
	}
	else{printf("couldnt open %s \n",filename_bin);loop_read = false;}
	index++;
}
for(int i = 0; i < nr_data; i++)
{
	if(ammount_for_improvement[i] != 0)
	{
		total_for_improvement[i]/=ammount_for_improvement[i];
		if(total_for_improvement[i] > 0.0001f){printf("%f\n",total_for_improvement[i]);}
	}
}

loop_read = true;
index = 0;
while(loop_read)
{
	{
	char filename_bin[256];
	sprintf(filename_bin, "%s_%i.results",responses_path, index);
	ifstream::pos_type fsize_bin;
	ifstream file_bin (filename_bin, ios::in|ios::binary|ios::ate);
	if (file_bin.is_open())
	{
		printf("Opening again: %s\n",filename_bin);
		fsize_bin = file_bin.tellg();
		file_bin.seekg (0, ios::beg);
		file_bin.read ((char *)tmp_improved, fsize_bin);
		file_bin.close();
		nr_data = fsize_bin/4;
		for(int i = 0; i < nr_data; i++)
		{
			if(tmp_improved[i]==-1){tmp_improved[i] = 0;}
		}
		char filename_bin_naive[256];
		sprintf(filename_bin_naive, "%s_naive_%i.results",responses_path, index);
		printf("Writing: %s\n",filename_bin_naive);
		ofstream outfile_bin (filename_bin_naive,ofstream::binary);
		outfile_bin.write ((char *)tmp_improved,fsize_bin);
		outfile_bin.close();
		{
		char filename_base[256];
		sprintf(filename_base, "%s_naive.base",responses_path);
		int output_base_size = 3;

		float * output_base = new float[output_base_size];
		output_base[0] = nr_data;
		output_base[1] = nr_regs;
		output_base[2] = target_object_size;
		ofstream outfile_base (filename_base,ofstream::binary);
		outfile_base.write ((char *)output_base,output_base_size*4);
		outfile_base.close();
		}
	}
	else{printf("couldnt open %s \n",filename_bin);loop_read = false;}
	}
	{
	char filename_bin[256];
	sprintf(filename_bin, "%s_%i.results",responses_path, index);
	ifstream::pos_type fsize_bin;
	ifstream file_bin (filename_bin, ios::in|ios::binary|ios::ate);
	if (file_bin.is_open())
	{
		fsize_bin = file_bin.tellg();
		file_bin.seekg (0, ios::beg);
		file_bin.read ((char *)tmp_improved, fsize_bin);
		file_bin.close();
		nr_data = fsize_bin/4;
		for(int i = 0; i < nr_data; i++)
		{
			if(tmp_improved[i]==-1){tmp_improved[i] = total_for_improvement[i];}
		}
		char filename_bin_improved[256];
		sprintf(filename_bin_improved, "%s_improved_%i.results",responses_path, index);
		printf("Writing: %s\n",filename_bin_improved);
		ofstream outfile_bin (filename_bin_improved,ofstream::binary);
		outfile_bin.write ((char *)tmp_improved,fsize_bin);
		outfile_bin.close();
		{
		char filename_base[256];
		sprintf(filename_base, "%s_improved.base",responses_path);
		int output_base_size = 3;

		float * output_base = new float[output_base_size];
		output_base[0] = nr_data;
		output_base[1] = nr_regs;
		output_base[2] = target_object_size;
		ofstream outfile_base (filename_base,ofstream::binary);
		outfile_base.write ((char *)output_base,output_base_size*4);
		outfile_base.close();
		}
	}
	else{printf("couldnt open %s \n",filename_bin);loop_read = false;}
	}

	index++;
}

/*






	char filename_base2[256];
	sprintf(filename_base2, "./output/v6.base");
	int output_base_size2 = 3;

	float * output_base2 = new float[output_base_size2];
	ifstream::pos_type fsize2;
	ifstream file_base2 (filename_base2, ios::in|ios::binary|ios::ate);
	if (file_base2.is_open())
	{
		fsize2 = file_base2.tellg();
		file_base2.seekg (0, ios::beg);
		file_base2.read ((char *)output_base2, fsize2);
		file_base2.close();
	}else
	{
		printf("couldnt open base\n");
	}

	int classifier_responses_saved 	= output_base2[0];
	int nr_of_patches 			= output_base2[1];
	target_object_size			= output_base2[2];
	delete[] output_base2;
	printf("classifier_responses_saved = %i\n",classifier_responses_saved);
	printf("nr_of_patches = %i\n",nr_of_patches);
	printf("target_object_size = %f\n",target_object_size);
	//exit(0);
	float ** responses = new float*[nr_of_patches+1];
	for(int i = 0; i < nr_of_patches+1;i++)
	{
		char filename_bin[256];
		sprintf(filename_bin, "./output/v6_%i.results", i);
		responses[i] = new float[classifier_responses_saved];
		ifstream::pos_type fsize_bin;
		ifstream file_bin (filename_bin, ios::in|ios::binary|ios::ate);
		if (file_bin.is_open())
		{
			fsize_bin = file_bin.tellg();
			file_bin.seekg (0, ios::beg);
			file_bin.read ((char *)responses[i], fsize_bin);
			file_bin.close();
		}
		else{printf("couldnt open %s \n",filename_bin);}
	}
	for(int i = 0; i < classifier_responses_saved;i++)
	{
		if(responses[0][i]!=0){
			printf("%i : %f \n",i,responses[0][i]);
		}
	}
	printf("\n");
	BoostedClassifier * boosted_classifier = new BoostedClassifier("./output/svm_classifier_v4_","./output/v6_");
	//BoostedClassifier * boosted_classifier = new BoostedClassifier(list_of_classifiers,responses,classifier_responses_saved,nr_of_patches);









	//BoostedClassifier * boosted_classifier = new BoostedClassifier("./output/b00sted9");
	//boosted_classifier->load("./output/b00sted2");
	m_classifier = boosted_classifier;//list_of_classifiers[0];//
	m_nextImageIndex = 0;
	while (readNextImageSet(image_rgb, image_depth, image_mask))
	{
		evaluate_image(m_classifier, m_IntegralImageFeature,image_rgb,image_depth, 1, 1);
		cvReleaseImage(&image_rgb);
		cvReleaseImage(&image_depth);
		cvReleaseImage(&image_mask);
	}
	printf("DONE!\n");//To have time to check for memleeks
	exit(0);
	*/

}

/* --------------------------------------------------------------------------*/
ContextLearner::~ContextLearner()
{
	// TODO Auto-generated destructor stub
}

}
;
